#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <signal.h>
#include <spdlog/spdlog.h>

#include <chrono>
#include <opencv2/opencv.hpp>
#include <thread>

#include "INIReader.h"
#include "flashai/commons/spdlog_utils.h"
#include "flashai/commons/visualize.h"
#include "flashros/commons/utils.h"
#include "flashros_commons/BoxesMsg.h"

void BoxesMsgConvertion(const flashros_commons::BoxesMsgConstPtr& msg,
                        std::vector<flashai::commons::Box>& boxes) {
  for (int i = 0; i < msg->num_objects; i++) {
    flashai::commons::Box box;
    box.x1 = msg->bboxes[i * 4];
    box.y1 = msg->bboxes[i * 4 + 1];
    box.x2 = msg->bboxes[i * 4 + 2];
    box.y2 = msg->bboxes[i * 4 + 3];
    box.score = msg->socres[i];
    box.distance = msg->distances[i];
    box.class_id = msg->classes[i];
    box.text = msg->texts[i];
    boxes.push_back(box);
  }
}

int main(int argc, char** argv) {
  // ros init
  flashros::commons::RosInit(argc, argv, "FlashROS_Fuse_Node");
  ros::NodeHandle nh;

  // get data root
  std::string data_root = flashros::commons::GetRosParam(nh, "data_root");

  // Read config file
  INIReader cfg_reader =
      *flashros::commons::InitConfig(nh, "flashros_fuse_config_path");

  // init spdlog
  flashai::utils::InitSpdLogger(cfg_reader);
  SPDLOG_INFO("Spdlog init successfully");

  // init visualizer
  std::string visualizer_section = "visualizer";
  auto visualizer = flashai::utils::DetectionVisualizer(
      cfg_reader, visualizer_section, data_root);

  // init params
  int max_fps = cfg_reader.GetInteger("base", "max_fps", 25);
  int min_inference_ms = (int)(1000 / max_fps);
  const int statistics_cnt =
      cfg_reader.GetInteger("base", "statistics_cnt", 100);
  bool enable_show = cfg_reader.GetBoolean("base", "enable_show", false);
  int img_height = cfg_reader.GetInteger("base", "output_height", 0);
  int img_width = cfg_reader.GetInteger("base", "output_width", 0);

  // read label file
  std::vector<std::string> object_label_names = {"human"};

  // create filter callback
  int iter_cnt = 0;
  auto previous = std::chrono::system_clock::now();
  auto callback_fn = [&](const sensor_msgs::ImageConstPtr& image,
                         const flashros_commons::BoxesMsgConstPtr& msg) {
    auto iter_start = std::chrono::system_clock::now();
    SPDLOG_DEBUG("start callback");

    // get frame
    cv::Mat frame = cv_bridge::toCvShare(image, "bgr8")->image;
    SPDLOG_DEBUG("Successfully get frame ({}, {})", frame.rows, frame.cols);

    // draw and resize to target size
    std::vector<flashai::commons::Box> boxes;
    BoxesMsgConvertion(msg, boxes);
    visualizer.Draw(frame, boxes);

    SPDLOG_DEBUG("Successfully draw {} boxes", boxes.size());

    // Resize image if necessary
    if (frame.rows != img_height || frame.cols != img_width) {
      cv::resize(frame, frame, cv::Size(img_width, img_height));
      SPDLOG_DEBUG("Successfully resize to ({}, {})", frame.rows, frame.cols);
    }

    if (enable_show) {
      cv::imshow("view", frame);
      cv::waitKey(1);
    }

    frame.release();
    // sleep
    int duration = (int)std::chrono::duration_cast<std::chrono::milliseconds>(
                       std::chrono::system_clock::now() - iter_start)
                       .count();
    if (duration < min_inference_ms) {
      std::this_thread::sleep_for(
          std::chrono::milliseconds(min_inference_ms - duration));
      SPDLOG_DEBUG("sleep {} ms", min_inference_ms - duration);
    } else {
      SPDLOG_DEBUG(
          "don't need to sleep since iter duration is {} ms and min inference "
          "ms is {}",
          duration, min_inference_ms);
    }

    auto end = std::chrono::system_clock::now();
    duration = (int)std::chrono::duration_cast<std::chrono::milliseconds>(
                   end - previous)
                   .count();
    SPDLOG_DEBUG("Fuse: num objects {}, fps {}", msg->num_objects,
                 1000. / duration);
    previous = end;
  };

  // read data from camera node and model prediction node
  message_filters::Subscriber<sensor_msgs::Image> image_sub(
      nh, cfg_reader.GetString("base", "camera_topic_name", ""), 100000);
  message_filters::Subscriber<flashros_commons::BoxesMsg> msg_sub(
      nh, cfg_reader.GetString("base", "msg_topic_name", ""), 1000000);
  SPDLOG_INFO("camera topic name: `{}`, msg topic name: `{}`",
              cfg_reader.GetString("base", "camera_topic_name", ""),
              cfg_reader.GetString("base", "msg_topic_name", ""));
  typedef message_filters::sync_policies::ApproximateTime<
      sensor_msgs::Image, flashros_commons::BoxesMsg>
      MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(30), image_sub,
                                                   msg_sub);
  sync.registerCallback(boost::bind<void>(callback_fn, _1, _2));
  ros::spin();
  return 0;
}
